#include "GPSData.h"

#include <stdlib.h>
#include <string>
#include <string.h>
#include <stdio.h>

int GPSData::ProcessGPGGA(char *sentence) 
{
	/*
	GPGGA Sentence format

	$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
		 |   |        |           |      | |   |     |      |   || |
		 |   |        |           |      | |   |     |      |   || checksum data
		 |   |        |           |      | |   |     |      |   ||
		 |   |        |           |      | |   |     |      |   2 empty fields
		 |   |        |           |      | |   |     |      |
		 |   |        |           |      | |   |     |    46.9,M Height of geoid (m) above WGS84 ellipsoid
		 |   |        |           |      | |   |     |
		 |   |        |           |      | |   |  545.4,M Altitude (m) above mean sea level
		 |   |        |           |      | |   |
		 |   |        |           |      | |  0.9 Horizontal dilution of position (HDOP)
		 |   |        |           |      | |
		 |   |        |           |      | 08 Number of satellites being tracked
		 |   |        |           |      |
		 |   |        |           |      1 Fix quality: 0 = invalid
		 |   |        |           |                     1 = GPS fix (SPS)
		 |   |        |           |                     2 = DGPS fix
		 |   |        |           |                     3 = PPS fix
		 |   |        |           |                     4 = Real Time Kinematic
		 |   |        |           |                     5 = Float RTK
		 |   |        |           |                     6 = estimated (dead reckoning) (2.3 feature)
		 |   |        |           |                     7 = Manual input mode
		 |   |        |           |                     8 = Simulation mode
		 |   |        |           |
		 |   |        |      01131.000,E Longitude 11 deg 31.000' E
		 |   |        |	 
		 |   |    4807.038,N Latitude 48 deg 07.038' N
		 |   |
		 |   123519 Fix taken at 12:35:19 UTC
		 |
		 GGA Global Positioning System Fix Data
	*/

	char *token = NULL;

	 token = strtok(sentence, ",");
    if (strcmp(token, "$GPGGA"))
        return -666; // Invalid sentence

	// Time
	token = strtok(NULL, ",");
	unsigned sec = atoi(token + 4);
	token[4] = '\0';
	unsigned min = atoi(token + 2);
	token[2] = '\0';
	unsigned hour = atoi(token);
	
	// Latitude
	token = strtok(NULL, ",");
	double latitude = atof(token);
	token = strtok(NULL, ",");
	if (*token == 'S')
		latitude *= -1;
	else if (*token != 'N')
		return -666;
	
    // Longitude
	token = strtok(NULL, ",");
	double longitude = atof(token);
	token = strtok(NULL, ",");
	if (*token == 'W')
		longitude *= -1;
	else if (*token != 'E')
		return -666;

	// GPS quality
	token = strtok(NULL, ",");
	unsigned fixQuality = atoi(token);
    
    // Satellites in use
	token = strtok(NULL, ",");
	unsigned numSatelitesInUse = atoi(token);

    // HDOP
    token = strtok(NULL, ",");
    double hdop = atoi(token);

    // Altitude
  	token = strtok(NULL, ",");
	double altitude = atof(token);
	token = strtok(NULL, ",");
	if (*token != 'M')
		return -666;

    // Height of geoid
  	token = strtok(NULL, ",");
	double geoidHeight = atof(token);
	token = strtok(NULL, ",");
	if (*token != 'M')
		return -666;

    // Empty field
    token = strtok(NULL, ",");
	if (*token != ' ')
		return -666;

    // Empty field
    token = strtok(NULL, ",");
	if (*token != ' ')
		return -666;

    // Set the values of m_GPSInfo
    m_hdop = hdop;
    m_geoidHeight = geoidHeight;
    m_latitude = latitude;
    m_longitude = longitude;
    m_altitude = altitude;
    m_fixQuality = fixQuality;
    m_numSatelitesInUse = numSatelitesInUse;
    m_numSentences++;

    return 0;
}

int GPSData::ProcessGPRMC(char *sentence) {
	/*
	GPRMC Sentence format

	$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
		|     |   |     |           |         |     |     |      |     |
		|     |   |     |           |         |     |     |      |    *6A Checksum data
		|     |   |     |           |         |     |     |      |
		|     |   |     |           |         |     |     |   003.1,W Magnetic Variation
		|     |   |     |           |         |     |     |
		|     |   |     |           |         |     |     230394 Date - 23rd of March 1994
		|     |   |     |           |         |     |
		|     |   |     |           |         |     084.4 Track angle in degrees
		|     |   |     |           |         |
		|     |   |     |           |      022.4 Speed over the ground in knots
		|     |   |     |           |
		|     |   |     |      01131.000,E Longitude 11 deg 31.000' E
		|     |   |     |
		|     |   | 4807.038,N Latitude 48 deg 07.038' N
		|     |   |    
		|     |   A Status A=active or V=Void
		|     |
		|     123519 Fix taken at 12:35:19 UTC
		|
		RMC Recommended Minimum sentence C
	*/

    char *token = NULL;

	token = strtok(sentence, ",");
    if (strcmp(token, "$GPRMC"))
        return -666; // Invalid sentence

	// Time
	token = strtok(NULL, ",");
	unsigned sec = atoi(token + 4);
	token[4] = '\0';
	unsigned min = atoi(token + 2);
	token[2] = '\0';
	unsigned hour = atoi(token);

    // Status
	token = strtok(NULL, ",");
	// TODO: What to do with it?
	if (*token != 'A' && *token != 'V')
		return -666;
	
	// Latitude
	token = strtok(NULL, ",");
	double latitude = atof(token);
	token = strtok(NULL, ",");
	if (*token == 'S')
		latitude *= -1;
	else if (*token != 'N')
		return -666;
	
    // Longitude
	token = strtok(NULL, ",");
	double longitude = atof(token);
	token = strtok(NULL, ",");
	if (*token == 'W')
		longitude *= -1;
	else if (*token != 'E')
		return -666;

    // Ground speed
	token = strtok(NULL, ",");
    double speedOverGround = atof(token);

    // Course over ground (degrees)
	token = strtok(NULL, ",");;
    double courseOverGround = atof(token);

    // Date
	token = strtok(NULL, ",");
	unsigned year = atoi(token + 4);
	token[4] = '\0';
	unsigned month = atoi(token + 2);
	token[2] = '\0';
	unsigned day = atoi(token);

    // Magnetic variation
    token = strtok(NULL, ",");
    double magneticVariation = atof(token);
    if (*token == 'W')
        magneticVariation = -magneticVariation;
    else if (*token != 'E' )
        return -666;

    // Set the values of m_GPSInfo
    m_courseOverGround = courseOverGround;
	m_speedOverGround = speedOverGround;
    m_magneticVariation = magneticVariation;
    m_latitude = latitude;
    m_longitude = longitude;
    
	m_numSentences++;

    return 0;
}
